#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <tf2/utils.h>
#include <angles/angles.h>
#include <cmath>
#include <csignal>

struct Point
{
    double x, y;
    Point() : x(0), y(0) {}
    Point(double _x, double _y) : x(_x), y(_y) {}
};

static double distance2D(const Point& a, const Point& b)
{
    double dx = a.x - b.x;
    double dy = a.y - b.y;
    return std::hypot(dx, dy);   // C++11 比 sqrt(dx*dx+dy*dy) 快且安全
}

class OdomDistanceNode
{
public:
    OdomDistanceNode() : nh_(), total_dist_(0.0), total_dist_speed_(0.0), first_frame_(true)
    {
        sub_ = nh_.subscribe("/odom", 1000, &OdomDistanceNode::odomCB, this);
        timer_ = nh_.createTimer(ros::Duration(0.1), &OdomDistanceNode::timerCB, this);
        ROS_INFO("开始用坐标法累计里程，等待 /odom ……");
    }

    ~OdomDistanceNode()
    {
        ROS_INFO("几何法最终里程 = %.3f m", getTotalDistance());
        ROS_INFO("速度法最终里程 = %.3f m", getTotalDistanceSpeed());
    }

    double getTotalDistance() const { return total_dist_*1.08; }
    double getTotalDistanceSpeed() const { return total_dist_speed_; }

private:
    ros::NodeHandle nh_;
    ros::Subscriber sub_;
    ros::Timer timer_;
    ros::Time last_receive_time_;
    Point last_pos_;
    double total_dist_;         // 几何法累计里程
    double total_dist_speed_;   // 速度法累计里程
    double total_dist_speed_x;   // 速度法累计里程
    double total_dist_speed_y;   // 速度法累计里程
    bool first_frame_;
    ros::Time last_stamp_;

    void timerCB(const ros::TimerEvent& e)
    {
        if (!first_frame_ && (ros::Time::now() - last_receive_time_).toSec() > 4.0)
        {
            ROS_WARN("数据接收超时，几何法里程 = %.3f m, 速度法里程 = %.3f m", 
                    getTotalDistance(), getTotalDistanceSpeed());
            ROS_INFO_THROTTLE(0.5, "last_pos_.x=%lf, last_pos_.y=%lf", last_pos_.x, last_pos_.y);
            last_receive_time_ = ros::Time::now();
        }
        else if (first_frame_)
        {
            // 首次数据还未接收，不发出超时警告
            last_receive_time_ = ros::Time::now();
        }
    }

    /* 回调：几何法累加里程 */
    void odomCB(const nav_msgs::Odometry::ConstPtr& msg)
    {
        const auto& p = msg->pose.pose.position;
        Point curr(p.x, p.y);
        ros::Time curr_stamp = msg->header.stamp;

        if (first_frame_)
        {
            first_frame_ = false;
            last_pos_ = curr;
            last_stamp_ = curr_stamp;
            ROS_INFO("首帧就位，初始距离 = 0 m");
            ROS_INFO_THROTTLE(0.5, "p.x=%lf, p.y=%lf", p.x, p.y);
            return;
        }

        /* 1. 时间差检查：防止 bag 跳时 */
        double dt = (curr_stamp - last_stamp_).toSec();
        if (dt <= 0.0 || dt > 1.0)   // 1 s 以上认为时间戳回卷或包不连续
        {
            ROS_WARN("时间戳异常 dt=%.3f s，跳过本帧", dt);
            last_pos_ = curr;
            last_stamp_ = curr_stamp;
            return;
        }

        /* 2. 距离差检查 */
        double delta_dist = distance2D(curr, last_pos_);
        if (delta_dist > 1.0)        // 1 m 一跳基本不可能
        {
            ROS_WARN("跳变过大 Δs=%.3f m，丢弃", delta_dist);
            last_pos_ = curr;
            last_stamp_ = curr_stamp;
            return;
        }

        /* 3. 角度差检查（可选） */
        double last_yaw = tf2::getYaw(msg->pose.pose.orientation);  // 需要 #include <tf2/utils.h>
        // 这里简化：如果你没装 tf2，可以注释掉本段
        static double last_yaw_last = last_yaw;
        double dyaw = angles::shortest_angular_distance(last_yaw_last, last_yaw);
        if (std::abs(dyaw) > M_PI_2)   // 90° 突变
        {
            ROS_WARN("角度跳变过大 Δyaw=%.2f°，丢弃", dyaw * 180 / M_PI);
            last_pos_ = curr;
            last_stamp_ = curr_stamp;
            return;
        }
        last_yaw_last = last_yaw;

        /* 4. 正式累计 */
        total_dist_ += delta_dist;
        
        // 速度法计算里程增量
        double speed_x = msg->twist.twist.linear.x;
        double speed_y = msg->twist.twist.linear.y;
        total_dist_speed_x = speed_x * dt;
        total_dist_speed_y = speed_y * dt;
        total_dist_speed_ += std::hypot(total_dist_speed_x, total_dist_speed_y);
        
        last_pos_ = curr;
        last_stamp_ = curr_stamp;
        last_receive_time_ = ros::Time::now();

        /* 5. 打印 */
        ROS_INFO_THROTTLE(0.5, "几何法里程 = %.3f m (Δs=%.3f m)", total_dist_*1.08, delta_dist);
        ROS_INFO_THROTTLE(0.5, "速度法里程 = %.3f m (x速度=%.3f m/s, x速度=%.3f m/s, dt=%.3f s)", 
                         total_dist_speed_, speed_x, speed_y, dt);
        ROS_INFO_THROTTLE(0.5, "位置: x=%lf, y=%lf", p.x, p.y);
    }
};

OdomDistanceNode* g_node_ptr = nullptr;  // 全局指针用于信号处理

void sigintHandler(int sig)
{
    if (g_node_ptr) {
        ROS_INFO("程序中断，最终里程 = %.3f m", g_node_ptr->getTotalDistance());
    }
    ros::shutdown();
}

int main(int argc, char** argv)
{
    setlocale(LC_CTYPE, "zh_CN.utf8");
    ros::init(argc, argv, "odom_sub");
    OdomDistanceNode node;
    g_node_ptr = &node;
    signal(SIGINT, sigintHandler);
    ros::spin();
    return 0;
}